#include "line_segment_detector.h"

#include <cassert>
#include <cmath>
#include <algorithm>
#include <cfloat>

namespace clsd {

enum CLSDModes {
  CLSD_REFINE_NONE = 0,
  CLSD_REFINE_STD  = 1,
  CLSD_REFINE_ADV  = 2,
};

#define NOTDEF      double(-1024.0)  // Label for pixels with undefined gradient.
#define NOTUSED     0                // Label for pixels not used in yet.
#define USED        1                // Label for pixels already used in detection.

constexpr double M_3_2_PI      = (3 * M_PI) / 2;
constexpr double M_2__PI       = (2 * M_PI);
constexpr double M_DEG_TO_RADS = M_PI / 180;

inline float fastAtan2( const float& y, const float& x ) {
    constexpr float atan2_p1 =  0.9997878412794807f  * (float)(180/M_PI);
    constexpr float atan2_p3 = -0.3258083974640975f  * (float)(180/M_PI);
    constexpr float atan2_p5 =  0.1555786518463281f  * (float)(180/M_PI);
    constexpr float atan2_p7 = -0.04432655554792128f * (float)(180/M_PI);
    float ax = std::abs(x), ay = std::abs(y);
    float a, c, c2;
    if( ax >= ay ) {
        c = ay/(ax + (float)DBL_EPSILON);
        c2 = c*c;
        a = (((atan2_p7*c2 + atan2_p5)*c2 + atan2_p3)*c2 + atan2_p1)*c;
    }
    else {
        c = ax/(ay + (float)DBL_EPSILON);
        c2 = c*c;
        a = 90.f - (((atan2_p7*c2 + atan2_p5)*c2 + atan2_p3)*c2 + atan2_p1)*c;
    }
    if( x < 0 ) a = 180.f - a;
    if( y < 0 ) a = 360.f - a;
    return a;
}

inline double distSq(const double& x1, const double& y1, const double& x2, const double& y2) {
    return (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
}

inline double dist(const double& x1, const double& y1, const double& x2, const double& y2) {
    return std::sqrt( distSq(x1, y1, x2, y2) );
}

inline double angle_diff_signed(const double& a, const double& b) {
    // Signed angle difference
    double diff = a - b;
    while(diff <= -M_PI) { diff += M_2__PI; };
    while(diff >   M_PI) { diff -= M_2__PI; };
    return diff;
}

inline double angle_diff(const double& a, const double& b) {
    // Absolute value angle difference
    return std::fabs( angle_diff_signed(a, b) );
}

inline bool double_equal(const double& a, const double& b) {
    // trivial case
    if (a == b) return true;
    constexpr double RELATIVE_ERROR_FACTOR = 100.0;
    double abs_diff = fabs(a - b);
    double aa = fabs(a);
    double bb = fabs(b);
    double abs_max = (aa > bb)? aa : bb;
    if (abs_max < DBL_MIN) abs_max = DBL_MIN;
    return (abs_diff / abs_max) <= (RELATIVE_ERROR_FACTOR * DBL_EPSILON);
}

inline double log_gamma_windschitl(const double& x) {
    return 0.918938533204673 + (x - 0.5) * log(x) - x + 0.5 * x * log( x * sinh(1 / x) + 1 / (810.0 * pow(x, 6.0)) );
}

inline double log_gamma_lanczos(const double& x) {
    /**
     *   Computes the natural logarithm of the absolute value of
     *   the gamma function of x using the Lanczos approximation.
     *   See http://www.rskey.org/gamma.htm
     */
    static double q[7] = { 75122.6331530, 80916.6278952, 36308.2951477, 8687.24529705, 1168.92649479, 83.8676043424, 2.50662827511 };
    double a = (x + 0.5) * log(x + 5.5) - (x + 5.5);
    double b = 0;
    for (int n = 0; n < 7; ++n) {
        a -= log(x + double(n));
        b += q[n] * pow(x, double(n));
    }
    return a + log(b);
}

inline double log_gamma(const double& x) {
    return x > 15.0? log_gamma_windschitl(x) : log_gamma_lanczos(x);
}

template<typename dType>
void gaussian_blur_2d_chw(dType* src, dType* dst, int channel, int imgHeight, int imgWidth, int ksizeX, int ksizeY, float sigmaX, float sigmaY) {
    if((ksizeX % 2 != 1) || (ksizeY % 2 != 1)) {
        assert(0 && "Error! Kernel size must be odd number for Gaussian blur.\n");
    }
    if(sigmaX <= 0.0f) {
        sigmaX = 0.3f * ((ksizeX - 1) * 0.5 - 1) + 0.8;
    }
    if(sigmaY <= 0.0f) {
        sigmaY = 0.3f * ((ksizeY - 1) * 0.5 - 1) + 0.8;
    }

    int midX = ksizeX / 2;
    int midY = ksizeY / 2;

    float *kernelX = new float[ksizeX];
    float sum = 0.0f;
    for(int j = 0; j <= ksizeX - 1; j++) {
        float val = 1.0 / (sqrt(2 * M_PI) * sigmaX) * exp(-((midX - j) * (midX - j)) / (2 * sigmaX * sigmaX));
        kernelX[j] = val;
        sum += val;
    }
    for(int j = 0; j <= ksizeX - 1; j++) {
        kernelX[j] /= sum;
    }

    float *kernelY = new float[ksizeY];
    if(ksizeX == ksizeY && sigmaX == sigmaY) {
        for(int i = 0; i < ksizeY; i++) {
            kernelY[i] = kernelX[i];
        }
    }
    else {
        float sum = 0.0f;
        for (int i = 0; i <= ksizeY - 1; i++) {
            float val = 1.0 / (sqrt(2 * M_PI) * sigmaY) *
                        exp(-((midY - i) * (midY - i)) / (2 * sigmaY * sigmaY));
            kernelY[i] = val;
            sum += val;
        }
        for (int i = 0; i <= ksizeY - 1; i++) {
            kernelY[i] /= sum;
        }
    }

    int pixelNum = imgWidth * imgHeight;
    float *midImg = new float[channel * pixelNum];

    for(int c = 0; c < channel; c++) {
        for(int h = 0; h < imgHeight; h++) {
            int idxY = c * pixelNum + h * imgWidth;
            for(int w = 0; w < imgWidth; w++) {
                float val = 0.0f;
                for(int k = 0; k < ksizeX; k++) {
                    int idx = w - (midX - k);
                    if(idx >=0 && idx < imgWidth) {
                        val += src[idxY + idx] * kernelX[k];
                    }
                }
                midImg[idxY + w] = val;
            }
        }
    }

    for(int c = 0; c < channel; c++) {
        for(int h = 0; h < imgHeight; h++) {
            int idxY = c * pixelNum + h * imgWidth;
            for(int w = 0; w < imgWidth; w++) {
                float val = 0.0f;
                for(int k = 0; k < ksizeY; k++) {
                    int idx = h - (midY - k);
                    if(idx >=0 && idx < imgHeight) {
                        val += midImg[c * pixelNum + idx * imgWidth + w] * kernelY[k];
                    }
                }
                dst[idxY + w] = val;
            }
        }
    }


    delete []kernelX;
    delete []kernelY;
    delete []midImg;
}

class CLSDImpl {

    struct RegionPoint {
        int             x;
        int             y;
        unsigned char * used;
        double          angle;
        double          modgrad;
    };

    struct Point {
        int x = 0;
        int y = 0;
        Point() = default;
        Point(const int& i_x, const int& i_y) : x(i_x), y(i_y) { }
    };

    struct NormPoint {
        Point p;
        int           norm;
    };

    struct Rect {
        double x1, y1, x2, y2;    // first and second point of the line segment
        double width;             // rectangle width
        double x, y;              // center of the rectangle
        double theta;             // angle
        double dx,dy;             // (dx,dy) is vector oriented as the line segment
        double prec;              // tolerance angle
        double p;                 // probability of a point with angle within 'prec'
    };

    struct Edge {
        Point p;
        bool  taken;
    };

public:

    CLSDImpl() = default;

    ~CLSDImpl() {
        if (_image_data) {
            free(_image_data); _image_data = NULL;
        }
        if (_blured_image_data && _SCALE != 1) {
            free(_blured_image_data); _blured_image_data = NULL;
        }
        if (_scaled_image_data && _SCALE != 1) {
            free(_scaled_image_data); _scaled_image_data = NULL;
        }
        if (_angles_data) {
            free(_angles_data); _angles_data = NULL;
        }
        if (_modgrad_data) {
            free(_modgrad_data); _modgrad_data = NULL;
        }
        if (_used_data) {
            free(_used_data); _used_data = NULL;
        }
    }

    void detect(
            const unsigned char *       i_image_data,
            const unsigned int&         i_image_h,
            const unsigned int&         i_image_w,
            std::vector<clsd::Vec4f>&   o_lines,
            std::vector<double> *       o_width = nullptr,
            std::vector<double> *       o_prec = nullptr,
            std::vector<double> *       o_nfa = nullptr
            ) {
        /**
         * Detect lines in the input image.
         *
         * @param i_image   A grayscale(CV_8UC1) input image.
         *                  If only a roi needs to be selected, use
         *                  lsd_ptr->detect(image(roi), ..., lines);
         *                  lines += Scalar(roi.x, roi.y, roi.x, roi.y);
         * @param i_image_h Height of input image;
         * @param i_image_w Width of input image;
         * @param o_lines   Return: A vector of Vec4i or Vec4f elements specifying the beginning and ending point of a line.
         *                          Where Vec4i/Vec4f is (x1, y1, x2, y2), point 1 is the start, point 2 - end.
         *                          Returned lines are strictly oriented depending on the gradient.
         * @param o_width   Return: Vector of widths of the regions, where the lines are found. E.g. Width of line.
         * @param o_prec    Return: Vector of precisions with which the lines are found.
         * @param o_nfa     Return: Vector containing number of false alarms in the line region, with precision of 10%.
         *                          The bigger the value, logarithmically better the detection.
         *                              * -1 corresponds to 10 mean false alarms
         *                              * 0 corresponds to 1 mean false alarm
         *                              * 1 corresponds to 0.1 mean false alarms
         *                          This vector will be calculated _only_ when the objects type is REFINE_ADV
         */

        if (i_image_h != _IMG_H || i_image_w != _IMG_W) {
            _IMG_H = i_image_h;
            _IMG_W = i_image_w;
            if (_image_data) {
                free(_image_data); _image_data = NULL;
            }
            if (_blured_image_data && _SCALE != 1) {
                free(_blured_image_data); _blured_image_data = NULL;
            }
            if (_scaled_image_data && _SCALE != 1) {
                free(_scaled_image_data); _scaled_image_data = NULL;
            }
            if (_angles_data) {
                free(_angles_data); _angles_data = NULL;
            }
            if (_modgrad_data) {
                free(_modgrad_data); _modgrad_data = NULL;
            }
            if (_used_data) {
                free(_used_data); _used_data = NULL;
            }
        }

        if (_image_data == NULL) {
            _image_data = (unsigned char *)malloc(_IMG_H * _IMG_W * sizeof (unsigned char));
        }
        memcpy(_image_data, i_image_data, _IMG_H * _IMG_W * sizeof (unsigned char));

        if (_SCALE == 1) {
            if (_scaled_image_data == NULL) {
                _scaled_image_data = _image_data;
            }
        }
        else {
            if (_blured_image_data == NULL) {
                _blured_image_data = (unsigned char *)malloc(_IMG_H * _IMG_W * sizeof (unsigned char));
            }
            memset(_blured_image_data, 0, _IMG_H * _IMG_W * sizeof (unsigned char));

            _SCALED_IMG_W = int(_SCALE * _IMG_W + 0.5);
            _SCALED_IMG_H = int(_SCALE * _IMG_H + 0.5);
            if (_scaled_image_data == NULL) {
                _scaled_image_data = (unsigned char *)malloc(_SCALED_IMG_H * _SCALED_IMG_W * sizeof (unsigned char));
            }
            memset(_scaled_image_data, 0, _SCALED_IMG_H * _SCALED_IMG_W * sizeof (unsigned char));


            // Gaussian Bluring ...
            const double sigma = (_SCALE < 1)?(_SIGMA_SCALE / _SCALE):(_SIGMA_SCALE);
            const double sprec = 3;
            const unsigned int h =  (unsigned int)(ceil(sigma * sqrt(2 * sprec * log(10.0))));
            gaussian_blur_2d_chw(_image_data, _blured_image_data, 1, _IMG_H, _IMG_W, 1 + 2 * h, 1 + 2 * h, sigma, sigma);

            // Scale image to needed size
            double _RECIP_SCALE = 1 / _SCALE;
            std::vector<int> idx_h(0);
            for (int y = 0; y < _SCALED_IMG_H; y++) {
                idx_h.emplace_back( std::min(int(y * _RECIP_SCALE), int(_IMG_H-1)) * _IMG_W );
            }
            std::vector<int> idx_w(0);
            for (auto x = 0; x < _SCALED_IMG_W; x++) {
                idx_w.emplace_back( int(x * _RECIP_SCALE) );
            }
            auto ptr_scaled_image_data = _scaled_image_data;
            for (int y = 0; y < _SCALED_IMG_H; y++) {
                auto ptr_blured_image_data_at_y = _blured_image_data + idx_h[y];
                for (auto x = 0; x < _SCALED_IMG_W; x++) {
                    *ptr_scaled_image_data++ = ptr_blured_image_data_at_y[idx_w[x]];
                }
            }
        }

        _W_NEEDED = o_width != nullptr;
        _P_NEEDED = o_prec != nullptr;
        if (_LSD_REFINE_MOD < CLSD_REFINE_ADV) {
            _N_NEEDED = false;
        }
        else {
            _N_NEEDED = o_nfa != nullptr;
        }

        std::vector<double> w, p, n;

        _flsd(o_lines, w, p, n);

        if (_W_NEEDED) *o_width = w;
        if (_P_NEEDED) *o_prec  = p;
        if (_N_NEEDED) *o_nfa   = n;

        // Clear used structures
        _ordered_points.clear();
    }

private:

    void _flsd(std::vector<clsd::Vec4f>& i_lines, std::vector<double>& i_widths, std::vector<double>& i_precisions, std::vector<double>& i_nfas) {
        /**
         * Detect lines in the whole input image.
         *
         * @param lines         Return: A vector of Vec4f elements specifying the beginning and ending point of a line.
         *                              Where Vec4f is (x1, y1, x2, y2), point 1 is the start, point 2 - end.
         *                              Returned lines are strictly oriented depending on the gradient.
         * @param widths        Return: Vector of widths of the regions, where the lines are found. E.g. Width of line.
         * @param precisions    Return: Vector of precisions with which the lines are found.
         * @param nfas          Return: Vector containing number of false alarms in the line region, with precision of 10%.
         *                              The bigger the value, logarithmically better the detection.
         *                                  * -1 corresponds to 10 mean false alarms
         *                                  * 0 corresponds to 1 mean false alarm
         *                                  * 1 corresponds to 0.1 mean false alarms
         */

        // Angle tolerance
        const double prec = M_PI * _ANG_TH / 180;
        const double p = _ANG_TH / 180;
        const double rho = _QUANT / sin(prec);    // gradient magnitude threshold

        _ll_angle(rho, _N_BINS);

        _LOG_NT = 5 * (log10(double(_SCALED_IMG_W)) + log10(double(_SCALED_IMG_H))) / 2 + log10(11.0);
        const size_t min_reg_size = size_t(-_LOG_NT/log10(p)); // minimal number of points in region that can give a meaningful event

        // Initialize region only when needed
        if (_used_data == NULL) {
            _used_data = (unsigned char *)malloc(_SCALED_IMG_H * _SCALED_IMG_W * sizeof(unsigned char));
        }
        memset(_used_data, 0, _SCALED_IMG_H * _SCALED_IMG_W * sizeof(unsigned char));

        std::vector<RegionPoint> reg;

        // Search for line segments
        for (size_t i = 0, points_size = _ordered_points.size(); i < points_size; ++i) {
            const Point& point = _ordered_points[i].p;
            if ((_used_data[point.y * _SCALED_IMG_W + point.x] == NOTUSED) && (_angles_data[point.y * _SCALED_IMG_W + point.x] != NOTDEF)) {
                double reg_angle;
                _region_grow(_ordered_points[i].p, reg, reg_angle, prec);

                // Ignore small regions
                if (reg.size() < min_reg_size) { continue; }

                // Construct rectangular approximation for the region
                Rect rec;
                _region2rect(reg, reg_angle, prec, p, rec);

                double log_nfa = -1;
                if (_LSD_REFINE_MOD > CLSD_REFINE_NONE) {
                    // At least REFINE_STANDARD lvl.
                    if (!_refine(reg, reg_angle, prec, p, rec, _DENSITY_TH)) { continue; }

                    if (_LSD_REFINE_MOD >= CLSD_REFINE_ADV) {
                        // Compute NFA
                        log_nfa = this->_rect_improve(rec);
                        if (log_nfa <= _LOG_EPS) { continue; }
                    }
                }
                // Found new line

                // Add the offset
                rec.x1 += 0.5; rec.y1 += 0.5;
                rec.x2 += 0.5; rec.y2 += 0.5;

                // scale the result values if a sub-sampling was performed
                if (_SCALE != 1) {
                    rec.x1 /= _SCALE; rec.y1 /= _SCALE;
                    rec.x2 /= _SCALE; rec.y2 /= _SCALE;
                    rec.width /= _SCALE;
                }

                //Store the relevant data
                i_lines.push_back(clsd::Vec4f(float(rec.x1), float(rec.y1), float(rec.x2), float(rec.y2)));
                if (_W_NEEDED) i_widths.push_back(rec.width);
                if (_P_NEEDED) i_precisions.push_back(rec.p);
                if (_N_NEEDED && _LSD_REFINE_MOD >= CLSD_REFINE_ADV) i_nfas.push_back(log_nfa);
            }
        }
    }

    void _ll_angle(const double& i_threshold, const unsigned int& i_n_bins) {
        /**
         * Finds the angles and the gradients of the image. Generates a list of pseudo ordered points.
         *
         * @param threshold      The minimum value of the angle that is considered defined, otherwise NOTDEF
         * @param n_bins         The number of bins with which gradients are ordered by, using bucket sort.
         * @param ordered_points Return: Vector of coordinate points that are pseudo ordered by magnitude.
         *                       Pixels would be ordered by norm value, up to a precision given by max_grad/n_bins.
         */

        //Initialize data
        if (_angles_data == NULL) {
            _angles_data = (double *)malloc(_SCALED_IMG_H * _SCALED_IMG_W * sizeof (double));
        }
        memset(_angles_data, 0, _SCALED_IMG_H * _SCALED_IMG_W * sizeof (double));

        if (_modgrad_data == NULL) {
            _modgrad_data = (double *)malloc(_SCALED_IMG_H * _SCALED_IMG_W * sizeof (double));
        }
        memset(_modgrad_data, 0, _SCALED_IMG_H * _SCALED_IMG_W * sizeof (double));

        // set down boundaries ...
        auto ptr_angles_data_last_row = _angles_data + (_SCALED_IMG_H - 1) * _SCALED_IMG_W;
        for (auto c = 0; c < _SCALED_IMG_W; c++) {
            ptr_angles_data_last_row[c] = NOTDEF;
        }
        // set right boundaries ...
        for (auto r = 0; r < _SCALED_IMG_H - 1; r++) {
            auto ptr = _angles_data + r * _SCALED_IMG_W;
            ptr[_SCALED_IMG_W - 1] = NOTDEF;
        }

        // Computing gradient for remaining pixels
        double max_grad = -1;
        for (int y = 0; y < _SCALED_IMG_H - 1; ++y) {
            const unsigned char * scaled_image_row = _scaled_image_data + y * _SCALED_IMG_W;
            const unsigned char * next_scaled_image_row = _scaled_image_data + y * _SCALED_IMG_W + _SCALED_IMG_W;
            double* angles_row = _angles_data + y * _SCALED_IMG_W;
            double* modgrad_row = _modgrad_data + y * _SCALED_IMG_W ;
            for (int x = 0; x < _SCALED_IMG_W-1; ++x) {
                int DA = next_scaled_image_row[x + 1] - scaled_image_row[x];
                int BC = scaled_image_row[x + 1] - next_scaled_image_row[x];
                int gx = DA + BC;    // gradient x component
                int gy = DA - BC;    // gradient y component
                double norm = std::sqrt((gx * gx + gy * gy) / 4.0); // gradient norm

                modgrad_row[x] = norm;    // store gradient

                if (norm <= i_threshold) { // norm too small, gradient no defined
                    angles_row[x] = NOTDEF;
                }
                else {
                    angles_row[x] = fastAtan2(float(gx), float(-gy)) * M_DEG_TO_RADS;  // gradient angle computation
                    if (norm > max_grad) { max_grad = norm; }
                }
            }
        }

        // Compute histogram of gradient values
        double bin_coef = (max_grad > 0) ? double(i_n_bins - 1) / max_grad : 0; // If all image is smooth, max_grad <= 0
        for (int y = 0; y < _SCALED_IMG_H - 1; ++y) {
            const double* modgrad_row = _modgrad_data + y * _SCALED_IMG_W; // _modgrad.ptr<double>(y);
            for (int x = 0; x < _SCALED_IMG_W - 1; ++x) {
                NormPoint _point;
                int i = int(modgrad_row[x] * bin_coef);
                // _point.p = Point(x, y);
                _point.p.x = x;
                _point.p.y = y;
                _point.norm = i;
                _ordered_points.push_back(_point);
            }
        }

        // Sort
        std::sort(_ordered_points.begin(), _ordered_points.end(), _compare_norm);
    }

    void _region_grow(const Point& i_s, std::vector<RegionPoint>& i_reg, double& i_reg_angle, const double& i_prec) {
        /**
         * Grow a region starting from point s with a defined precision,
         * returning the containing points size and the angle of the gradients.
         *
         * @param s         Starting point for the region.
         * @param reg       Return: Vector of points, that are part of the region
         * @param reg_angle Return: The mean angle of the region.
         * @param prec      The precision by which each region angle should be aligned to the mean.
         */

        i_reg.clear();

        // Point to this region
        RegionPoint seed;
        seed.x = i_s.x;
        seed.y = i_s.y;
        seed.used = _used_data + i_s.y * _SCALED_IMG_W + i_s.x;
        i_reg_angle = _angles_data[i_s.y * _SCALED_IMG_W + i_s.x];
        seed.angle = i_reg_angle;
        seed.modgrad = _modgrad_data[i_s.y * _SCALED_IMG_W + i_s.x];
        i_reg.push_back(seed);

        float sumdx = float(std::cos(i_reg_angle));
        float sumdy = float(std::sin(i_reg_angle));
        *seed.used = USED;

        //Try neighboring regions
        for (size_t i = 0;i<i_reg.size();i++) {
            const RegionPoint& rpoint = i_reg[i];
            int xx_min = std::max(rpoint.x - 1, 0), xx_max = std::min(rpoint.x + 1, _SCALED_IMG_W - 1);
            int yy_min = std::max(rpoint.y - 1, 0), yy_max = std::min(rpoint.y + 1, _SCALED_IMG_H - 1);
            for (int yy = yy_min; yy <= yy_max; ++yy) {
                unsigned char * used_row = _used_data + yy * _SCALED_IMG_W; // _used.ptr<unsigned char>(yy);
                const double* angles_row = _angles_data + yy * _SCALED_IMG_W; // _angles.ptr<double>(yy);
                const double* modgrad_row = _modgrad_data + yy * _SCALED_IMG_W; // _modgrad.ptr<double>(yy);
                for (int xx = xx_min; xx <= xx_max; ++xx) {
                    unsigned char& is_used = used_row[xx];
                    if (is_used != USED && (_isAligned(xx, yy, i_reg_angle, i_prec))) {
                        const double& angle = angles_row[xx];
                        // Add point
                        is_used = USED;
                        RegionPoint region_point;
                        region_point.x = xx;
                        region_point.y = yy;
                        region_point.used = &is_used;
                        region_point.modgrad = modgrad_row[xx];
                        region_point.angle = angle;
                        i_reg.push_back(region_point);

                        // Update region's angle
                        sumdx += cos(float(angle));
                        sumdy += sin(float(angle));
                        // reg_angle is used in the isAligned, so it needs to be updates?
                        i_reg_angle = fastAtan2(sumdy, sumdx) * M_DEG_TO_RADS;
                    }
                }
            }
        }

    }

    void _region2rect(const std::vector<RegionPoint>& reg, const double reg_angle, const double prec, const double p, Rect& rec) const {
        /**
         * Finds the bounding rotated rectangle of a region.
         *
         * @param reg       The region of points, from which the rectangle to be constructed from.
         * @param reg_angle The mean angle of the region.
         * @param prec      The precision by which points were found.
         * @param p         Probability of a point with angle within 'prec'.
         * @param rec       Return: The generated rectangle.
         */

        double x = 0, y = 0, sum = 0;
        for (size_t i = 0; i < reg.size(); ++i) {
            const RegionPoint& pnt = reg[i];
            const double& weight = pnt.modgrad;
            x += double(pnt.x) * weight;
            y += double(pnt.y) * weight;
            sum += weight;
        }

        // Weighted sum must differ from 0
        assert(sum > 0);

        x /= sum;
        y /= sum;

        double theta = _get_theta(reg, x, y, reg_angle, prec);

        // Find length and width
        double dx = cos(theta);
        double dy = sin(theta);
        double l_min = 0, l_max = 0, w_min = 0, w_max = 0;

        for (size_t i = 0; i < reg.size(); ++i) {
            double regdx = double(reg[i].x) - x;
            double regdy = double(reg[i].y) - y;

            double l = regdx * dx + regdy * dy;
            double w = -regdx * dy + regdy * dx;

            if (l > l_max) l_max = l;
            else if (l < l_min) l_min = l;
            if (w > w_max) w_max = w;
            else if (w < w_min) w_min = w;
        }

        // Store values
        rec.x1 = x + l_min * dx;
        rec.y1 = y + l_min * dy;
        rec.x2 = x + l_max * dx;
        rec.y2 = y + l_max * dy;
        rec.width = w_max - w_min;
        rec.x = x;
        rec.y = y;
        rec.theta = theta;
        rec.dx = dx;
        rec.dy = dy;
        rec.prec = prec;
        rec.p = p;

        // Min width of 1 pixel
        if (rec.width < 1.0) rec.width = 1.0;
    }

    double _get_theta(const std::vector<RegionPoint>& reg, const double& x, const double& y, const double& reg_angle, const double& prec) const {
        /**
         * Compute region's angle as the principal inertia axis of the region.
         * @return          Regions angle.
         */

        double Ixx = 0.0;
        double Iyy = 0.0;
        double Ixy = 0.0;

        // Compute inertia matrix
        for (size_t i = 0; i < reg.size(); ++i) {
            const double& regx = reg[i].x;
            const double& regy = reg[i].y;
            const double& weight = reg[i].modgrad;
            double dx = regx - x;
            double dy = regy - y;
            Ixx += dy * dy * weight;
            Iyy += dx * dx * weight;
            Ixy -= dx * dy * weight;
        }

        // Check if inertia matrix is null
        assert(!(double_equal(Ixx, 0) && double_equal(Iyy, 0) && double_equal(Ixy, 0)));

        // Compute smallest eigenvalue
        double lambda = 0.5 * (Ixx + Iyy - sqrt((Ixx - Iyy) * (Ixx - Iyy) + 4.0 * Ixy * Ixy));

        // Compute angle
        double theta = (fabs(Ixx)>fabs(Iyy))?
                        double(fastAtan2(float(lambda - Ixx), float(Ixy))):
                        double(fastAtan2(float(Ixy), float(lambda - Iyy))); // in degs
        theta *= M_DEG_TO_RADS;

        // Correct angle by 180 deg if necessary
        if (angle_diff(theta, reg_angle) > prec) { theta += M_PI; }

        return theta;
    }

    bool _refine(std::vector<RegionPoint>& reg, double reg_angle, const double prec, double p, Rect& rec, const double& density_th) {
        /**
         * An estimation of the angle tolerance is performed by the standard deviation of the angle at points
         * near the region's starting point. Then, a new region is grown starting from the same point, but using the
         * estimated angle tolerance. If this fails to produce a rectangle with the right density of region points,
         * 'reduce_region_radius' is called to try to satisfy this condition.
         */

        double density = double(reg.size()) / (dist(rec.x1, rec.y1, rec.x2, rec.y2) * rec.width);

        if (density >= density_th) { return true; }

        // Try to reduce angle tolerance
        double xc = double(reg[0].x);
        double yc = double(reg[0].y);
        const double& ang_c = reg[0].angle;
        double sum = 0, s_sum = 0;
        int n = 0;

        for (size_t i = 0; i < reg.size(); ++i) {
            *(reg[i].used) = NOTUSED;
            if (dist(xc, yc, reg[i].x, reg[i].y) < rec.width) {
                const double& angle = reg[i].angle;
                double ang_d = angle_diff_signed(angle, ang_c);
                sum += ang_d;
                s_sum += ang_d * ang_d;
                ++n;
            }
        }
        double mean_angle = sum / double(n);
        // 2 * standard deviation
        double tau = 2.0 * sqrt((s_sum - 2.0 * mean_angle * sum) / double(n) + mean_angle * mean_angle);

        // Try new region
        _region_grow(Point(reg[0].x, reg[0].y), reg, reg_angle, tau);

        if (reg.size() < 2) { return false; }

        _region2rect(reg, reg_angle, prec, p, rec);
        density = double(reg.size()) / (dist(rec.x1, rec.y1, rec.x2, rec.y2) * rec.width);

        if (density < density_th) {
            return _reduce_region_radius(reg, reg_angle, prec, p, rec, density, density_th);
        }
        else {
            return true;
        }
    }

    bool _reduce_region_radius(std::vector<RegionPoint>& reg, double reg_angle, const double prec, double p, Rect& rec, double density, const double& density_th) {
        /**
         * Reduce the region size, by elimination the points far from the starting point, until that leads to
         * rectangle with the right density of region points or to discard the region if too small.
         */

        // Compute region's radius
        double xc = double(reg[0].x);
        double yc = double(reg[0].y);
        double radSq1 = distSq(xc, yc, rec.x1, rec.y1);
        double radSq2 = distSq(xc, yc, rec.x2, rec.y2);
        double radSq = radSq1 > radSq2 ? radSq1 : radSq2;

        while(density < density_th) {
            radSq *= 0.75*0.75; // Reduce region's radius to 75% of its value
            // Remove points from the region and update 'used' map
            for (size_t i = 0; i < reg.size(); ++i) {
                if ( distSq(xc, yc, double(reg[i].x), double(reg[i].y)) > radSq ) {
                    // Remove point from the region
                    *(reg[i].used) = NOTUSED;
                    std::swap(reg[i], reg[reg.size() - 1]); reg.pop_back(); // fast delete the retion point
                    --i; // To avoid skipping one point
                }
            }

            if ( reg.size() < 2 ) { return false; }

            // Re-compute rectangle
            _region2rect( reg ,reg_angle, prec, p, rec );

            // Re-compute region points density
            density = double(reg.size()) / (dist(rec.x1, rec.y1, rec.x2, rec.y2) * rec.width);
        }

        return true;
    }

    double _rect_improve(Rect& rec) const {
        /**
         * Try some rectangles variations to improve NFA value. Only if the rectangle is not meaningful (i.e., log_nfa <= log_eps).
         * @return      The new NFA value.
         */

        double delta = 0.5;
        double delta_2 = delta / 2.0;

        double log_nfa = this->_rect_nfa(rec);

        if (log_nfa > _LOG_EPS) return log_nfa; // Good rectangle

        // Try to improve
        // Finer precision
        Rect r = Rect(rec); // Copy
        for (int n = 0; n < 5; ++n) {
            r.p /= 2;
            r.prec = r.p * M_PI;
            double log_nfa_new = this->_rect_nfa(r);
            if (log_nfa_new > log_nfa) {
                log_nfa = log_nfa_new;
                rec = Rect(r);
            }
        }
        if (log_nfa > _LOG_EPS) return log_nfa;

        // Try to reduce width
        r = Rect(rec);
        for (unsigned int n = 0; n < 5; ++n) {
            if ((r.width - delta) >= 0.5) {
                r.width -= delta;
                double log_nfa_new = this->_rect_nfa(r);
                if (log_nfa_new > log_nfa) {
                    rec = Rect(r);
                    log_nfa = log_nfa_new;
                }
            }
        }
        if (log_nfa > _LOG_EPS) return log_nfa;

        // Try to reduce one side of rectangle
        r = Rect(rec);
        for (unsigned int n = 0; n < 5; ++n) {
            if ((r.width - delta) >= 0.5) {
                r.x1 += -r.dy * delta_2;
                r.y1 +=  r.dx * delta_2;
                r.x2 += -r.dy * delta_2;
                r.y2 +=  r.dx * delta_2;
                r.width -= delta;
                double log_nfa_new = this->_rect_nfa(r);
                if (log_nfa_new > log_nfa) {
                    rec = Rect(r);
                    log_nfa = log_nfa_new;
                }
            }
        }
        if (log_nfa > _LOG_EPS) return log_nfa;

        // Try to reduce other side of rectangle
        r = Rect(rec);
        for (unsigned int n = 0; n < 5; ++n) {
            if ((r.width - delta) >= 0.5) {
                r.x1 -= -r.dy * delta_2;
                r.y1 -=  r.dx * delta_2;
                r.x2 -= -r.dy * delta_2;
                r.y2 -=  r.dx * delta_2;
                r.width -= delta;
                double log_nfa_new = this->_rect_nfa(r);
                if (log_nfa_new > log_nfa) {
                    rec = Rect(r);
                    log_nfa = log_nfa_new;
                }
            }
        }
        if (log_nfa > _LOG_EPS) return log_nfa;

        // Try finer precision
        r = Rect(rec);
        for (unsigned int n = 0; n < 5; ++n) {
            if ((r.width - delta) >= 0.5) {
                r.p /= 2;
                r.prec = r.p * M_PI;
                double log_nfa_new = this->_rect_nfa(r);
                if (log_nfa_new > log_nfa) {
                    rec = Rect(r);
                    log_nfa = log_nfa_new;
                }
            }
        }

        return log_nfa;
    }

    double _rect_nfa(const Rect& rec) const {
        /**
         * Calculates the number of correctly aligned points within the rectangle.
         * @return      The new NFA value.
         */

        int total_pts = 0, alg_pts = 0;
        double half_width = rec.width / 2.0;
        double dyhw = rec.dy * half_width;
        double dxhw = rec.dx * half_width;

        Edge ordered_x[4];
        Edge* min_y = &ordered_x[0];
        Edge* max_y = &ordered_x[0]; // Will be used for loop range

        ordered_x[0].p.x = int(rec.x1 - dyhw); ordered_x[0].p.y = int(rec.y1 + dxhw); ordered_x[0].taken = false;
        ordered_x[1].p.x = int(rec.x2 - dyhw); ordered_x[1].p.y = int(rec.y2 + dxhw); ordered_x[1].taken = false;
        ordered_x[2].p.x = int(rec.x2 + dyhw); ordered_x[2].p.y = int(rec.y2 - dxhw); ordered_x[2].taken = false;
        ordered_x[3].p.x = int(rec.x1 + dyhw); ordered_x[3].p.y = int(rec.y1 - dxhw); ordered_x[3].taken = false;

        std::sort(ordered_x, ordered_x + 4, [](const Edge& a, const Edge& b)->bool { return a.p.x == b.p.x? (a.p.y < b.p.y) : (a.p.x < b.p.x); } );

        // Find min y. And mark as taken. find max y.
        for (unsigned int i = 1; i < 4; ++i) {
            if (min_y->p.y > ordered_x[i].p.y) {min_y = &ordered_x[i]; }
            if (max_y->p.y < ordered_x[i].p.y) {max_y = &ordered_x[i]; }
        }
        min_y->taken = true;

        // Find leftmost untaken point;
        Edge* leftmost = 0;
        for (unsigned int i = 0; i < 4; ++i) {
            if (!ordered_x[i].taken) {
                if (!leftmost)  { // if uninitialized
                    leftmost = &ordered_x[i];
                }
                else if (leftmost->p.x > ordered_x[i].p.x) {
                    leftmost = &ordered_x[i];
                }
            }
        }
        assert(leftmost != NULL);
        leftmost->taken = true;

        // Find rightmost untaken point;
        Edge* rightmost = 0;
        for (unsigned int i = 0; i < 4; ++i) {
            if (!ordered_x[i].taken) {
                if (!rightmost) { // if uninitialized
                    rightmost = &ordered_x[i];
                }
                else if (rightmost->p.x < ordered_x[i].p.x) {
                    rightmost = &ordered_x[i];
                }
            }
        }
        assert(rightmost != NULL);
        rightmost->taken = true;

        // Find last untaken point;
        Edge* tailp = 0;
        for (unsigned int i = 0; i < 4; ++i) {
            if (!ordered_x[i].taken) {
                if (!tailp) { // if uninitialized
                    tailp = &ordered_x[i];
                }
                else if (tailp->p.x > ordered_x[i].p.x) {
                    tailp = &ordered_x[i];
                }
            }
        }
        assert(tailp != NULL);
        tailp->taken = true;

        double flstep = (min_y->p.y != leftmost->p.y) ?  (min_y->p.x - leftmost->p.x)  / (min_y->p.y - leftmost->p.y)  : 0; //first left step
        double slstep = (leftmost->p.y != tailp->p.x) ?  (leftmost->p.x - tailp->p.x)  / (leftmost->p.y - tailp->p.x)  : 0; //second left step

        double frstep = (min_y->p.y != rightmost->p.y) ? (min_y->p.x - rightmost->p.x) / (min_y->p.y - rightmost->p.y) : 0; //first right step
        double srstep = (rightmost->p.y != tailp->p.x) ? (rightmost->p.x - tailp->p.x) / (rightmost->p.y - tailp->p.x) : 0; //second right step

        double lstep = flstep, rstep = frstep;

        double left_x = min_y->p.x, right_x = min_y->p.x;

        // Loop around all points in the region and count those that are aligned.
        int min_iter = min_y->p.y;
        int max_iter = max_y->p.y;
        for (int y = min_iter; y <= max_iter; ++y) {
            if (y < 0 || y >= _SCALED_IMG_H) continue;

            for (int x = int(left_x); x <= int(right_x); ++x) {
                if (x < 0 || x >= _SCALED_IMG_W) continue;

                ++total_pts;
                if (_isAligned(x, y, rec.theta, rec.prec)) {
                    ++alg_pts;
                }
            }

            if (y >= leftmost->p.y) { lstep = slstep; }
            if (y >= rightmost->p.y) { rstep = srstep; }

            left_x += lstep;
            right_x += rstep;
        }

        return _nfa(total_pts, alg_pts, rec.p);
    }

    double _nfa(const int& n, const int& k, const double& p) const {
        /**
         * Computes the NFA values based on the total number of points, points that agree.
         * n, k, p are the binomial parameters.
         * @return      The new NFA value.
         */

        // Trivial cases
        if (n == 0 || k == 0) { return -_LOG_NT; }
        if (n == k) { return -_LOG_NT - double(n) * log10(p); }

        double p_term = p / (1 - p);

        double log1term = (double(n) + 1) - log_gamma(double(k) + 1)
                    - log_gamma(double(n-k) + 1)
                    + double(k) * log(p) + double(n-k) * log(1.0 - p);
        double term = exp(log1term);

        if (double_equal(term, 0)) {
            if (k > n * p) return -log1term / M_LN10 - _LOG_NT;
            else return -_LOG_NT;
        }

        // Compute more terms if needed
        double bin_tail = term;
        double tolerance = 0.1; // an error of 10% in the result is accepted
        for (int i = k + 1; i <= n; ++i) {
            double bin_term = double(n - i + 1) / double(i);
            double mult_term = bin_term * p_term;
            term *= mult_term;
            bin_tail += term;
            if (bin_term < 1) {
                double err = term * ((1 - pow(mult_term, double(n-i+1))) / (1 - mult_term) - 1);
                if (err < tolerance * fabs(-log10(bin_tail) - _LOG_NT) * bin_tail) break;
            }

        }
        return -log10(bin_tail) - _LOG_NT;
    }

    bool _isAligned(int x, int y, const double& theta, const double& prec) const {
        /**
         * Is the point at place 'address' aligned to angle theta, up to precision 'prec'?
         * @return      Whether the point is aligned.
         */

        if (x < 0 || y < 0 || x >= _SCALED_IMG_W || y >= _SCALED_IMG_H) { return false; }
        const double& a = _angles_data[y * _SCALED_IMG_W + x]; // _angles.at<double>(y, x);
        if (a == NOTDEF) { return false; }

        // It is assumed that 'theta' and 'a' are in the range [-pi,pi]
        double n_theta = theta - a;
        if (n_theta < 0) { n_theta = -n_theta; }
        if (n_theta > M_3_2_PI) {
            n_theta -= M_2__PI;
            if (n_theta < 0) n_theta = -n_theta;
        }

        return n_theta <= prec;
    }

    static inline bool _compare_norm( const NormPoint& n1, const NormPoint& n2 ) {
        // Compare norm
        return n1.norm > n2.norm;
    }

protected:
    unsigned char *         _image_data         { NULL };
    unsigned char *         _blured_image_data  { NULL };
    unsigned char *         _scaled_image_data  { NULL };
    double *                _angles_data        { NULL };
    double *                _modgrad_data       { NULL };
    unsigned char *         _used_data          { NULL };
    std::vector<NormPoint>  _ordered_points     { {} };

    // please set the lsd paramters here ...
    double                  _LOG_NT;
    int                     _IMG_W              { 0 };
    int                     _IMG_H              { 0 };
    int                     _SCALED_IMG_W       { 0 };
    int                     _SCALED_IMG_H       { 0 };
    bool                    _W_NEEDED           { false };
    bool                    _P_NEEDED           { false };
    bool                    _N_NEEDED           { false };
    const double            _SCALE              { 0.8 };
    const int               _LSD_REFINE_MOD     { CLSD_REFINE_STD };
    const double            _SIGMA_SCALE        { 0.6 };
    const double            _QUANT              { 2.0 };
    const double            _ANG_TH             { 22.5 };
    const double            _LOG_EPS            { 0 };
    const double            _DENSITY_TH         { 0.7 };
    const int               _N_BINS             { 1024 };
};

CLSD::CLSD() {
    _m = new CLSDImpl();
}

CLSD::~CLSD() {
    delete static_cast<CLSDImpl *>(_m);
}

std::vector<Vec4f> CLSD::Detect(const unsigned char *i_edge, const unsigned int i_edge_h, const unsigned int i_edge_w) const {
    std::vector<Vec4f> lines(0);
    static_cast<CLSDImpl *>(_m)->detect(i_edge, i_edge_h, i_edge_w, lines);
    return lines;
}

}
